#include <Arduino.h>
#include "FOC_Motor.h"
#include "sensor.h"

AS5600 test_sensor;
FOC_Motor test_motor(32, 33, 25, 7, 12, 10, &test_sensor);

void setup()
{
  Serial.begin(115200);
  test_motor.calibrate_sensor();

  vTaskDelay(3000);
  test_sensor.read_angle_task_start();
  vTaskDelay(10000);
  test_sensor.read_angle_task_end();
  vTaskDelay(1000);
  test_motor.closeloop_pos_start(PI);
  vTaskDelay(30000);
  test_motor.closeloop_pos_end();
}

void loop()
{
  while (1)
  {
    vTaskDelay(10);
  }
}